/*  This file is part of the KITT project
    Purpose: This file provides functions for adjusting direction and position of the KITT
    using serial commands assoicated with the Roboteq AX2550 motor controller.
    Functions:
    Roboteq(int port)-This function initializes the comport connected to the Roboteq.
    roboSetAngle(int angle)-This function turns the vehicle to a desired angle using a digital compass.
    roboMove(float nextposition)-This function moves the vehicle a certain distance.

    Program Depends on:
    Arduino-These files provide functions to retrieve distance and heading information.
    kittrs232-These files provide a set of serial functions for the KITT devices.
*/
#include <iostream>
#include <cmath>
#include <cstdlib>
#include "kittrs232.h"
#include <string>
#include <sstream>
#include <iomanip>
#include "roboteq.h"
#include "Arduino.h"

#define PI 3.1415926536


using namespace std;
Arduino arduino(17);
RoboTeq::RoboTeq(int port) {
    this->port = port;
    OpenComport(port,9600,C7e1);
}

void RoboTeq::roboSetAngle(int angle) {
    int frontang,curang;
    frontang=round(angle)-90;
    curang=arduino.ArdGetHeading();
    int nextheading = 0;
    nextheading=frontang+curang;
    if (frontang>0)
    {
        cprintf(port,"!B35\r");
    } else if (frontang<0)
    {
        cprintf(port,"!b35\r");
    } else
    {
        cprintf(port,"!B00\r");
    }

    cprintf(port,"!A10\r");
    while (curang!=nextheading);
    {
       curang=arduino.ArdGetHeading();
    }
    cprintf(port,"!B00\r");
    cprintf(port,"!A00\r");
    curheading=curang;
}

void RoboTeq::roboMove(float nextposition){
    float travdis,oldendDistance;
    int angDiff,nextangle;
    float startdis=arduino.ArdGetDistance();
    cprintf(port,"!A10\r");
    while (curposition<nextposition);
    {
        curposition=arduino.ArdGetDistance();
    }
    cprintf(port,"!B00\r");
    cprintf(port,"!A00\r");
    travdis=curposition-startdis;
    oldendDistance=endDistance;
    if (abs(curheading-endAngle)>180){
    angDiff=360-abs(curheading-endAngle);
    } else {
    angDiff=abs(curheading-endAngle);
    }
    if (angDiff==0) {
    endDistance=oldendDistance-travdis;
    } else {

    endDistance=sqrt(oldendDistance*oldendDistance+travdis*travdis-2*oldendDistance*travdis*cosf(angDiff*PI/180));
    nextangle=round((180/PI)*(PI-asin(oldendDistance*(sin(angDiff*PI/180)/endDistance))));
    }
    if ((curheading-endAngle)<0){
    endAngle=curheading-nextangle;
    }else {
    endAngle=curheading+nextangle;
    }
}
